/*********************************************************************************
  *  @Copyright(C),1996-2021, Company
  *  @FileName: kalman.c
  *  @Author: MO
  *  @Version: V1.0
  *  @Date: 2022-01-07 18:33:25
  *  @Description:
  *
  *
  *  @History:
       1.Date:
         Author:
         Modification:
       2.
**********************************************************************************/


/***************************************Includes***********************************/
#include "kalman.h"


/***************************************Macros***********************************/
// #define


/***************************************Statement***********************************/


/***************************************Variables***********************************/



/***************************************Functions***********************************/
// 1. 结构体类型定义


// 2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP_TypeDef KFP_flow = {
    .LastP = 0.02,
    .Now_P = 0,
    .out = 0,
    .Kg = 0,
    .Q = 0.000009,
    .R = 0.05};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float kalmanFilter(KFP_TypeDef *kfp, float input) {
    // 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    // 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
    // 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out); // 因为这一次的预测值就是上一次的输出值
    // 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

/**
 *调用卡尔曼滤波器 实践
 */

float get_kalman(float input) {
    float kalman_flow = 0;
    kalman_flow = kalmanFilter(&KFP_flow, input);
    return kalman_flow;
}
/**
 * @brief 滑动平均
 * @param  type
 * @param  d  数据
 * @return float
 * @date 2022-01-08 10:01:42
 * @note null
 */
float moving_avr(MOVE_AVR_TypeDef *type, float d) {
    float sum = 0;
    char count;
#if PRINT_LOG
    if (type->index_cnt >= MOVE_AVR_MAX) {
        PRINT("moving avr overflow!!!\n");
        return 0;
    }
#endif
    type->flow_buf[type->index_cnt++] = d;
    if (type->index_max == type->index_cnt)
        type->index_cnt = 0;
    for (count = 0; count < type->index_max; count++)
        sum += type->flow_buf[count];
    return sum / type->index_max;
}

/* [] END OF FILE */
